//
//  main.cpp
//  TrackingSim
//
//  Created by Garrett Manka on 11/24/11.
//  Copyright (c) 2011. All rights reserved.
//

#include <Engine.h>
#include <tracking/Kalman.h>
#include <iostream>
#include <iomanip>
#include <math.h>

void filtertest()
{
	KalmanFilter<6> k;
    Matrix<6,1> state(0.0);
    Matrix<6,6> cov(0.0);
    Matrix<1,1> z;
    Matrix<1,6> h(0.0);
    Matrix<1,1> r(1000.0);
    
    double dt = 5.0;
    
    state(0,0) = 1.0;
    state(1,0) = 0.0;
    state(2,0) = 0.0;
    state(3,0) = 1.0;
    state(4,0) = 0.0;
    state(5,0) = 0.0;
    
    cov.set_to_identity();
    cov(0,0) = 10.0;
    cov(1,1) = 10.0;
    cov(2,2) = 10.0;
    
    Matrix<6,1> state_est = state;
    Matrix<1,1> z_est;
    
    for(int i = 0 ; i < 5 ; ++i )
    {    
        state(0,0) = state(0,0) + dt * state(3,0);
        state(1,0) = state(1,0) + dt * state(4,0);
        state(2,0) = state(2,0) + dt * state(5,0);
        
        if(i==0) state_est = state;
        
        h(0,0) = 2.0 * state_est(0,0);
        h(0,1) = 2.0 * state_est(1,0);
        h(0,2) = 2.0 * state_est(2,0);
        
        //Measurement
        z(0,0) = state(0,0) * state(0,0) + state(1,0) * state(1,0) + state(2,0) * state(2,0);
        
        //Estimated measurement
        z_est(0,0) = state_est(0,0) * state_est(0,0) + state_est(1,0) * state_est(1,0) + state_est(2,0) * state_est(2,0);
        
        //Residual
        Matrix<1,1> y = z - z_est;
        
        k.setState(state_est);
        k.setCovariance(cov);
        
        k.update(y, h, r);
        
        state_est = k.getState();
        cov = k.getCovariance();
        
        std::cout<<"Meas Range: " << sqrt( z(0,0) ) << " Est Range: " << sqrt( state_est(0,0) * state_est(0,0) + state_est(1,0) * state_est(1,0) + state_est(2,0) * state_est(2,0) ) << std::endl;
        
//        std::cout<<std::setw(10)<<std::setprecision(4)<<"state: " << state_est(0,0) << "\t\t" << state_est(1,0) << "\t\t" << state_est(2,0)
//                                                      <<"\t\t"    << state_est(3,0) << "\t\t" << state_est(4,0) << "\t\t" << state_est(5,0) << std::endl;
//        std::cout<<std::setw(10)<<std::setprecision(4)<<"cov: " << cov(0,0) << "\t\t" << cov(1,1) << "\t\t" << cov(2,2)
//                                                    <<"\t\t"    << cov(3,3) << "\t\t" << cov(4,4) << "\t\t" << cov(5,5) << std::endl;
    }
}

int main (int argc, char * argv[])
{
    //filtertest();
    
    Engine::getEngine()->load();
    Engine::getEngine()->start(argc, argv);
    
    return 0;
}

